#!/usr/bin/python
# -*- coding: UTF-8 -*-

from MiniBotControlLib import MiniBotControlLib
import time
import math


def msSleep(ms):
    time.sleep(0.001 * ms)

class CarRobot:
    #******************定义用户参数******************
    W_SPEED = 80  #传入舵机速度，1（MIN）至100（MAX）
    W_ANGLE = 90  #传入小车自转角度，1（MIN）至360（MAX）
    PI = 3.14159
    SPEED_ROUND = 40
    
    #******************定义硬件参数******************/
    DIAM_RUB_WHEEL = 70 #橡胶轮的直径mm #原70，实测66
    DIAM_ALL_WHEEL = 80 #全向轮的直径mm
    DIAM_MAI_WHEEL = 80 #麦克纳姆轮的直径mm
    
    RAD_TWO_WHEEL = 160    #二轮的整车半径mm
    RAD_THREE_WHEEL = 160  #三轮的整车半径mm
    RAD_FOUR_A_WHEEL = 160  #四向轮整车半径mm
    RAD_FOUR_M_WHEEL = 160  #麦轮的整车半径mm
    RAD_FOUR_B_WHEEL = 160  #四并轮整车半径mm
    RAD_CAR_R = 160        #小车（所有轮）整车半径mm
    
    #小车一圈的周长
    CIRCLE_TWO = (2 * PI * RAD_TWO_WHEEL)
    CIRCLE_THREE = (2 * PI * RAD_THREE_WHEEL)
    CIRCLE_FOUR_A = (2 * PI * RAD_FOUR_A_WHEEL)
    CIRCLE_FOUR_M = (2 * PI * RAD_FOUR_M_WHEEL)
    CIRCLE_FOUR_B = (2 * PI * RAD_FOUR_B_WHEEL)
    
    #轮子一圈的周长
    CIRCLE_WHEEL_RUB = (PI * DIAM_RUB_WHEEL)
    CIRCLE_WHEEL_ALL = (PI * DIAM_ALL_WHEEL)
    CIRCLE_WHEEL_MAI = (PI * DIAM_MAI_WHEEL)
    
    #小车周长与轮子周长的比例,即轮子要走几圈，小车才走完一圈
    Two_Scale = (CIRCLE_TWO / CIRCLE_WHEEL_RUB)
    Thr_Scale = (CIRCLE_THREE / CIRCLE_WHEEL_ALL)
    FourA_Scale = (CIRCLE_FOUR_A / CIRCLE_WHEEL_ALL)
    FourM_Scale = (CIRCLE_FOUR_M / CIRCLE_WHEEL_MAI)
    FourB_Scale = (CIRCLE_FOUR_B / CIRCLE_WHEEL_ALL)
    
    ONE_ROUND_TIME = 3160 #I模块在40的速度下转一圈估计的时间ms，速度暂时固定40
    
    #小车旋转1°对应的i模块旋转时间ms
    Two_Ms = (Two_Scale * ONE_ROUND_TIME / 360.0) #小车自转一周耗费的时间/360°，可得出转一度需要多少ms
    Thr_Ms = (Thr_Scale * ONE_ROUND_TIME / 360.0)
    FourA_Ms = (FourA_Scale * ONE_ROUND_TIME / 360.0)
    FourM_Ms = (FourM_Scale * ONE_ROUND_TIME / 360.0)
    FourB_Ms = (FourB_Scale * ONE_ROUND_TIME / 360.0)
  
    #标记运行状态
    is_running = False

    #ID列表
    wheel_id1 = 0
    wheel_id2 = 0
    wheel_id3 = 0
    wheel_id4 = 0
    Mode = 2

    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,mclib):
        self.lib = mclib
    # 更新小车的速度
    def update_speed(self, speed):
        self.speed = speed

    #绑定ID与模型:ids F,B,l1,l2,l3,l4,r1,r2,r3,r4
    def bindIds(self,ids,mode):
        self.Mode = mode
        if(self.Mode == 2):
            self.wheel_id1 = ids[3]
            self.wheel_id2 = ids[7]
        if(self.Mode == 3):
            self.wheel_id1 = ids[0]
            self.wheel_id2 = ids[4]
            self.wheel_id3 = ids[8]
            self.wheel_id4 = 0
        if(self.Mode == 4):
            self.wheel_id1 = ids[3]
            self.wheel_id2 = ids[0]
            self.wheel_id3 = ids[7]
            self.wheel_id4 = ids[1]
        if(self.Mode == 5 or mode == 6):
            self.wheel_id1 = ids[2]
            self.wheel_id2 = ids[6]
            self.wheel_id3 = ids[5]
            self.wheel_id4 = ids[9]

        self.lib.SetMotorMode(self.wheel_id1,1)
        self.lib.SetMotorMode(self.wheel_id2,1)
        self.lib.SetMotorMode(self.wheel_id3,1)
        self.lib.SetMotorMode(self.wheel_id4,1)

    def setWheelSpeed(self,id,speed):
        self.lib.SetMotorTargetSpeed(id,speed)

    #************* 两轮 - Start

    def goForward_2wheel(self):
        print("两轮向前走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        pass

    def goBack_2wheel(self):
        print("两轮向后走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        pass

    def turnLeft_2wheel(self):
        print("两轮左转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        pass

    def turnRight_2wheel(self):
        print("两轮右转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        pass

    #************* 两轮 - End



    #************* 三轮 - Start

    def goForward_3wheel(self):
        print("三轮向前走...")
        self.setWheelSpeed(self.wheel_id1,0)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        pass

    def goBack_3wheel(self):
        print("三轮向后走...")
        self.setWheelSpeed(self.wheel_id1,0)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        pass

    def turnLeft_3wheel(self):
        print("三轮左转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        pass

    def turnRight_3wheel(self):
        print("三轮右转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        pass

    #************* 三轮 - End



    #************* 四向轮 - Start

    def goForward_4wheel(self):
        print("四向轮向前走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,0)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,0)
        pass

    def goBack_4wheel(self):
        print("四向轮向后走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,0)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,0)
        pass

    def turnLeft_4wheel(self):
        print("四向轮左转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED * -1)
        pass

    def turnRight_4wheel(self):
        print("四向轮右转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED)
        pass

    #************* 四向轮 - End



    #************* 四麦轮 - Start

    def goForward_4Mwheel(self):
        print("四麦克朗姆轮向前走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED * -1)
        pass

    def goBack_4Mwheel(self):
        print("四麦克朗姆轮向后走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED)
        pass

    def turnLeft_4Mwheel(self):
        print("四麦克朗姆轮左转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED * -1)
        pass

    def turnRight_4Mwheel(self):
        print("四麦克朗姆轮右转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED)
        pass

    def runLeft_4Mwheel(self):
        print("四麦克朗姆轮向左走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED)
        pass

    def runRight_4Mwheel(self):
        print("四麦克朗姆轮向右走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED * -1)
        pass

    #************* 四麦轮 - End




    #************* 四并轮 - Start

    def goForward_4Nwheel(self):
        print("四并橡胶轮向前走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED)
        pass

    def goBack_4Nwheel(self):
        print("四并橡胶轮向后走...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED * -1)
        pass

    def turnLeft_4Nwheel(self):
        print("四并橡胶轮左转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED * -1)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED * -1)
        pass

    def turnRight_4Nwheel(self):
        print("四并橡胶轮右转...")
        self.setWheelSpeed(self.wheel_id1,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id2,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id3,self.W_SPEED)
        self.setWheelSpeed(self.wheel_id4,self.W_SPEED)
        pass

    #************* 四麦轮 - End



    #************* 旋转指定角度 - Start

    def rotate_2wheel(self,angle):        
        if(angle > 0):
            print("两轮左旋转："+str(angle))
            self.turnLeft_2wheel()
        else:
            print("两轮右旋转："+str(angle))
            self.turnRight_2wheel()

        sl = self.Two_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.stop()
        pass

    def rotate_3wheel(self,angle):
        if(angle > 0):
            print("三轮左旋转："+str(angle))
            self.turnLeft_3wheel()
        else:
            print("三轮右旋转："+str(angle))
            self.turnRight_3wheel()

        sl = self.Thr_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.stop()
        pass

    def rotate_4wheel(self,angle):
        if(angle > 0):
            print("四向轮左旋转："+str(angle))
            self.turnLeft_4wheel()
        else:
            print("四向轮右旋转："+str(angle))
            self.turnRight_4wheel()

        sl = self.FourA_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.stop()
        pass

    def rotate_4Mwheel(self,angle):
        if(angle > 0):
            print("四麦克朗姆轮左旋转："+str(angle))
            self.turnLeft_4Mwheel()
        else:
            print("四麦克朗姆轮右旋转："+str(angle))
            self.turnRight_4Mwheel()

        sl = self.FourM_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.stop()
        pass

    def rotate_4Nwheel(self,angle):
        if(angle > 0):
            print("四并轮左旋转："+str(angle))
            self.turnLeft_4Nwheel()
        else:
            print("四并轮左旋转："+str(angle))
            self.turnRight_4Nwheel()

        sl = self.FourB_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.stop()
        pass

    #************* 旋转指定角度 - End


    def stop_wheel(self):
        print("停止")
        self.setWheelSpeed(self.wheel_id1,0)
        self.setWheelSpeed(self.wheel_id2,0)
        self.setWheelSpeed(self.wheel_id3,0)
        self.setWheelSpeed(self.wheel_id4,0)
        pass


    #************* 全局控制 - Start

    def stop(self):
        self.is_running = False
        self.stop_wheel()

    def goForward(self):
        self.is_running = True
        if(self.Mode == 2):
            self.goForward_2wheel()
        if(self.Mode == 3):
            self.goForward_3wheel()
        if(self.Mode == 4):
            self.goForward_4wheel()
        if(self.Mode == 5):
            self.goForward_4Mwheel()
        if(self.Mode == 6):
            self.goForward_4Nwheel()
        pass

    def goBack(self):
        self.is_running = True
        if(self.Mode == 2):
            self.goBack_2wheel()
        if(self.Mode == 3):
            self.goBack_3wheel()
        if(self.Mode == 4):
            self.goBack_4wheel()
        if(self.Mode == 5):
            self.goBack_4Mwheel()
        if(self.Mode == 6):
            self.goBack_4Nwheel()
        pass

    def turnLeft(self):
        self.is_running = True
        if(self.Mode == 2):
            self.turnLeft_2wheel()
        if(self.Mode == 3):
            self.turnLeft_3wheel()
        if(self.Mode == 4):
            self.turnLeft_4wheel()
        if(self.Mode == 5):
            self.turnLeft_4Mwheel()
        if(self.Mode == 6):
            self.turnLeft_4Nwheel()
        pass

    def turnRight(self):
        self.is_running = True
        if(self.Mode == 2):
            self.turnRight_2wheel()
        if(self.Mode == 3):
            self.turnRight_3wheel()
        if(self.Mode == 4):
            self.turnRight_4wheel()
        if(self.Mode == 5):
            self.turnRight_4Mwheel()
        if(self.Mode == 6):
            self.turnRight_4Nwheel()
        pass

    def runLeft(self):
        if(self.Mode == 5):
            self.is_running = True
            self.runLeft_4Mwheel()
        pass

    def runRight(self):
        if(self.Mode == 5):
            self.is_running = True
            self.runRight_4Mwheel()
        pass

    def rotate(self,angle):
        self.is_running = True
        if(self.Mode == 2):
            self.rotate_2wheel(angle)
        if(self.Mode == 3):
            self.rotate_3wheel(angle)
        if(self.Mode == 4):
            self.rotate_4wheel(angle)
        if(self.Mode == 5):
            self.rotate_4Mwheel(angle)
        if(self.Mode == 6):
            self.rotate_4Nwheel(angle)
        pass

    #************* 全局控制 - End



    #************* For ROS DRIVER - Start

    #设置速度（米/秒）
    def setRunningSpeedWithMS(self,ms):

        #I空载 最大速度为3400/S
        MAX_STEP = 3400
        #ms = ms * 1.25
        #MAX_STEP=3050 #负载观察值

        #获取轮子周长
        c = self.CIRCLE_WHEEL_RUB  #橡胶轮
        if(self.Mode == 5):
            c = self.CIRCLE_WHEEL_MAI   #麦克朗姆轮
        if(self.Mode == 3 or self.Mode == 4):
            c = self.CIRCLE_WHEEL_ALL   #麦克朗姆轮

        #计算目标圈数
        mms = ms * 100 * 10.0
        qcount = mms / c

        #计算每秒步数
        #I模块一圈为4095,空载最大步数速度为3400
        setps = qcount * 4095
        validSetp = min(abs(setps),MAX_STEP)
        speedPercent = validSetp * 100.0 / MAX_STEP

        maxSpeed = MAX_STEP / 4096.0 * c / 1000
        print("Max speed is:" + str(maxSpeed) + "m/s")
        print("Now is setting speed (" + str(speedPercent) + "% steps=" + str(validSetp) + "):" + str(maxSpeed * speedPercent / 100) + "m/s")
        if(ms < 0):
           speedPercent = -speedPercent

        self.W_SPEED = int(speedPercent)
        return
    
    
     #设置速度（弧度/秒）
     #最大值：(3400/4095*70*3.14)/(160*3.14*2)*(2*3.14)=1.1405982905983
    def setRunningSpeedWithRadS(self,rads):

        #I空载 最大速度为3400/S
        MAX_STEP = 3400
        
        #获取轮子周长
        wheelc = self.CIRCLE_WHEEL_RUB  #橡胶轮
        if(self.Mode == 5):
            wheelc = self.CIRCLE_WHEEL_MAI   #麦克朗姆轮
        if(self.Mode == 3 or self.Mode == 4):
            wheelc = self.CIRCLE_WHEEL_ALL   #全向轮

        #获取整车周长
        c = self.RAD_CAR_R * self.PI * 2 

        #每秒需走多少周长
        s = rads / (2 * self.PI) * c

        #转为即每秒需走多少圈
        rounds = s / wheelc

        #转换为每秒需走多少步
        steps = rounds * 4095

        #校正最步数值
        validSetp = min(abs(steps),MAX_STEP) #为实际表现系数
        speedPercent = validSetp * 100.0 / MAX_STEP

        self.W_SPEED = int(speedPercent)
        return;

    
    #************* For ROS DRIVER - End

